//////////////////////////////////////////////////////////////////////
//
//	ZETeam source code 
//	Copyright (c) ZETeam 2011-..
//	
//	File: CQuat.cpp
//  Description: Quaternion classes. Based on Id Software source code.
// 
//	History:
//	- February 27, 2011: Created by Max Streltsov
//
//////////////////////////////////////////////////////////////////////

#include "CQuat.h"
#pragma hdrstop


#include "CAngles.h"
#include "CRotation.h"

/*
=====================
CQuat::ToAngles
=====================
*/
CAngles CQuat::toAngles( void ) const {
	return toMat3().toAngles();
}

/*
=====================
CQuat::ToRotation
=====================
*/
CRotation CQuat::toRotation( void ) const {
	CVec3 vec;
	float angle;

	vec.x = x;
	vec.y = y;
	vec.z = z;
	angle = CZMath::ACos( w );
	if ( angle == 0.0f ) {
		vec.set( 0.0f, 0.0f, 1.0f );
	} else {
		//vec *= (1.0f / sin( angle ));
		vec.normalize();
		vec.fixDegenerateNormal();
		angle *= 2.0f * CZMath::M_RAD2DEG;
	}
	return CRotation( vec3_origin, vec, angle );
}

/*
=====================
CQuat::ToMat3
=====================
*/
CMat3 CQuat::toMat3( void ) const {
	CMat3	mat;
	float	wx, wy, wz;
	float	xx, yy, yz;
	float	xy, xz, zz;
	float	x2, y2, z2;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	xy = x * y2;
	xz = x * z2;

	yy = y * y2;
	yz = y * z2;
	zz = z * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
	mat[ 0 ][ 1 ] = xy - wz;
	mat[ 0 ][ 2 ] = xz + wy;

	mat[ 1 ][ 0 ] = xy + wz;
	mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
	mat[ 1 ][ 2 ] = yz - wx;

	mat[ 2 ][ 0 ] = xz - wy;
	mat[ 2 ][ 1 ] = yz + wx;
	mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );

	return mat;
}

/*
=====================
CQuat::ToMat4
=====================
*/
CMat4 CQuat::toMat4( void ) const {
	return toMat3().toMat4();
}

/*
=====================
CQuat::ToCQuat
=====================
*/
CCQuat CQuat::toCQuat( void ) const {
	if ( w < 0.0f ) {
		return CCQuat( -x, -y, -z );
	}
	return CCQuat( x, y, z );
}

/*
============
CQuat::ToAngularVelocity
============
*/
CVec3 CQuat::toAngularVelocity( void ) const {
	CVec3 vec;

	vec.x = x;
	vec.y = y;
	vec.z = z;
	vec.normalize();
	return vec * CZMath::ACos( w );
}

/*
=============
CQuat::ToString
=============
*/
//const char *CQuat::ToString( int precision ) const {
//	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
//}

/*
=====================
CQuat::Slerp

Spherical linear interpolation between two quaternions.
=====================
*/
CQuat &CQuat::slerp( const CQuat &from, const CQuat &to, float t ) {
	CQuat	temp;
	float	omega, cosom, sinom, scale0, scale1;

	if ( t <= 0.0f ) {
		*this = from;
		return *this;
	}

	if ( t >= 1.0f ) {
		*this = to;
		return *this;
	}

	if ( from == to ) {
		*this = to;
		return *this;
	}

	cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
	if ( cosom < 0.0f ) {
		temp = -to;
		cosom = -cosom;
	} else {
		temp = to;
	}

	if ( ( 1.0f - cosom ) > 1e-6f ) {
#if 0
		omega = acos( cosom );
		sinom = 1.0f / sin( omega );
		scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
		scale1 = sin( t * omega ) * sinom;
#else
		scale0 = 1.0f - cosom * cosom;
		sinom = CZMath::InvSqrt( scale0 );
		omega = CZMath::ATan16( scale0 * sinom, cosom );
		scale0 = CZMath::Sin16( ( 1.0f - t ) * omega ) * sinom;
		scale1 = CZMath::Sin16( t * omega ) * sinom;
#endif
	} else {
		scale0 = 1.0f - t;
		scale1 = t;
	}

	*this = ( scale0 * from ) + ( scale1 * temp );
	return *this;
}

/*
=============
CCQuat::ToAngles
=============
*/
CAngles CCQuat::toAngles( void ) const {
	return toQuat().toAngles();
}

/*
=============
CCQuat::ToRotation
=============
*/
CRotation CCQuat::toRotation( void ) const {
	return toQuat().toRotation();
}

/*
=============
CCQuat::ToMat3
=============
*/
CMat3 CCQuat::toMat3( void ) const {
	return toQuat().toMat3();
}

/*
=============
CCQuat::ToMat4
=============
*/
CMat4 CCQuat::toMat4( void ) const {
	return toQuat().toMat4();
}

/*
=============
CCQuat::ToString
=============
*/
//const char *CCQuat::toString( int precision ) const {
//	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
//}
